import math
import random
import time
import numpy as np
from Node import Node
from point import point
from plotmaze import *


class RRT:  # 定义A*算法中的函数
    def __init__(self, obstacle, startnode, endnode, length, width, step, maxangle, Pg):
        self.leaf = []
        self.path = []

        self.mazeObstacle = obstacle  # 地图信息，记录不可行点
        self.root = startnode
        self.endNode = endnode
        self.length = length
        self.width = width
        self.step = step
        self.maxangle = maxangle
        self.Pg = Pg
        self.randnode = None
        self.currNode = None
        self.tempPG = self.Pg - 3 / 100

    def minDisnode(self):  # 计算leaf节点中的最小距离的节点
        if len(self.leaf) == 0:
            plotmaze(self.mazeObstacle, (self.root.point.x, self.root.point.y),
                     (self.endNode.point.x, self.endNode.point.y), self.length, self.width)
            return None
        else:
            minNode = self.leaf[0]
            for tempNode in self.leaf:
                if tempNode.distance < minNode.distance:
                    minNode = tempNode
            return minNode

    def getRandnode(self):
        #print("try")
        percent = random.random()
        if percent >= self.Pg:
            self.randnode = Node(point(random.uniform(0, self.length - 1), random.uniform(0, self.width - 1)))
        else:
            self.randnode = self.endNode

    def getLeafdis(self):
        for tempNode in self.leaf:
            if self.randnode is not None:
                tempNode.getDis(self.randnode)

    def isAccess(self, thisPoint):  # 判断point是否可通行
        if thisPoint.x < 0 or thisPoint.y < 0:  # 低于下界
            return False
        if thisPoint.x >= self.length - 1 or thisPoint.y >= self.width - 1:  # 高于上界
            return False
        for i in self.mazeObstacle:  # 处于障碍物内
            if pow(i[0] - thisPoint.x, 2) + pow(i[1] - thisPoint.y, 2) <= pow(i[2], 2):
                return False
        return True

    def Heron(self, a, b, c):
        p = (a + b + c) / 2
        S = math.sqrt(p * (p - a) * (p - b) * (p - c))
        h = 2 * S / a
        return h

    def isSafe(self, newPoint, nearNode):
        newNear = self.step
        for i in self.mazeObstacle:
            newDx = newPoint.x - i[0]
            newDy = newPoint.y - i[1]
            nearDx = nearNode.point.x - i[0]
            nearDy = nearNode.point.y - i[1]

            newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
            nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))
            h = self.Heron(newNear, newO, nearO) # 海伦公式
            cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
            cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear) # 余弦定理

            if h <= i[2] and cosnew > 0 and cosnear > 0:
                return False

        return True

    def isLast(self, thisNode):
        thisNode.getDis(self.endNode)
        if thisNode.distance <= self.step * 3:
            print('end')
            return True
        else:
            return False

    def newLeaf(self):
        if len(self.leaf) == 0:
            self.leaf.append(self.root)
            self.currNode = self.root
        else:
            self.getRandnode()  # 获取rand节点
            self.getLeafdis()  # 计算所有叶子节点到rand节点的距离
            nearNode = self.minDisnode()  # 找到距离rand节点距离最近的叶子节点
            randNode = self.randnode  # 将rand节点非self化
            # nearNode.getDis(randNode)  # 再计算一次距离rand节点距离，应该可删

            gapX = randNode.point.x - nearNode.point.x
            gapY = randNode.point.y - nearNode.point.y  # 两点坐标差
            gapD = nearNode.distance  # 两点距离
            sin = gapX / gapD
            cos = gapY / gapD  # 两点角度

            newX = nearNode.point.x + sin * self.step
            newY = nearNode.point.y + cos * self.step
            newPoint = point(newX, newY)  # 按一定步长沿该方向找到新节点

            if self.isAccess(newPoint) is not True:  # 新节点是否可达
                #print("not access")
                if self.Pg >= 0.5:
                    self.Pg = self.Pg - 1 / 100
                return

            if self.isSafe(newPoint, nearNode) is not True:  # 新旧节点之间是否有障碍物
                #print("not safe")
                if self.Pg >= 0.5:
                    self.Pg = self.Pg - 1 / 100
                return

            # if cos <= math.cos(self.maxangle / 180 * math.pi):  # 该路线角度是否过大
            newNode = Node(newPoint)
            newNode.ancientNode(nearNode)
            self.leaf.append(newNode)
            self.currNode = newNode
            if self.Pg < self.tempPG:
                self.Pg = self.Pg + 3 / 100
            print("right")
            plotAnimation(self.mazeObstacle, self.leaf, newPoint, nearNode.point, self.root.point, self.endNode.point, self.length, self.width)


    def run(self):
        temp = 0
        fig = plt.figure(1)
        while True:
            temp = temp + 1
            self.newLeaf()
            #plt.show()
            plt.pause(2)
            plt.close('all')
            if self.isLast(self.currNode) is True:
                print("last")
                self.endNode.ancient = self.currNode
                self.endNode.point.x = self.endNode.point.x - 1
                self.endNode.point.y = self.endNode.point.y - 1
                tempNode = self.endNode
                while True:
                    self.path.append(tempNode)
                    if tempNode.ancient != None:
                        tempNode = tempNode.ancient
                    else:
                        return self.path
            elif len(self.leaf) >= self.length * self.width * 0.1:
                return False




